#include "mainwindow.h"
#include "ui_mainwindow.h"


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    this->setWindowTitle(QString("ServoTest V0.1"));
    this->setWindowFlags(this->windowFlags() & ~Qt::WindowMaximizeButtonHint);
    this->setFixedSize(this->width(), this->height());
    ui->tableWidgetSlaves->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);

    ecatMaster = new ECatDrv::EthercatMaster;
    ServoLowUpdateTimer = new QTimer();

    if (ecatMaster->NetCardScan(m_netCard))
    {
        for (size_t i = 0; i < m_netCard.size(); ++i)
        {
            ui->comboBox_NetCards->addItem(QString(m_netCard[i].name.c_str()));
        }
    }
    else
    {
        int result = QMessageBox::warning(this, "错误", "未扫描到网卡，是否重试？", QMessageBox::Yes, QMessageBox::No);
        while (result == QMessageBox::Yes && !ecatMaster->NetCardScan(m_netCard))
        {
            result = QMessageBox::warning(this, "错误", "未扫描到网卡，是否重试？", QMessageBox::Yes, QMessageBox::No);
        }
    }

    connect(ServoLowUpdateTimer,SIGNAL(timeout()),this, SLOT(timerServoLowUpdateSlot()));
    connect(ui->pB_connect, SIGNAL(clicked()), this, SLOT(pBConnectSlot()));
    connect(ui->pB_SetOp, SIGNAL(clicked()), this, SLOT(pBSetOpSlot()));
    connect(ui->pB_SetSafeOp, SIGNAL(clicked()), this, SLOT(pBSetSafeOpSlot()));
    connect(ui->pB_servoClearFault, SIGNAL(clicked()), this, SLOT(pBServoClearFaultSlot()));
    connect(ui->pB_servoEnabled, SIGNAL(clicked()), this, SLOT(pBServoEnabledSlot()));
    connect(ui->pB_Jog, SIGNAL(clicked()), this, SLOT(pBJogSlot()));

    isServoInited = false;
    ServoLowUpdateTimer->start(500);                // 200ms刷新一次
}

MainWindow::~MainWindow()
{
    delete ui;
    delete ServoLowUpdateTimer;
    delete ecatMaster;
}

void MainWindow::pBConnectSlot(void)
{
    int res;
    if(!ecatMaster->m_masterInited)
    {
        res = ecatMaster->MasterInit(m_netCard[ui->comboBox_NetCards->currentIndex()].id, m_slave, m_servo, m_safety);

        if(0 == res)
        {
            ui->tableWidgetSlaves->setRowCount(0);
            ui->tableWidgetSlaves->clearContents();
            SetLog("info",QString("EtherCAT initialized successfully. find %1 slaves.").arg(m_slave.size()));
            int DriveNum = 0;
            for(size_t i = 0; i < m_slave.size(); i++)
            {
                SetLog("info",QString("Slaves %1 info:").arg(i));
                SetLog("info",QString("\t eep_man = %1.").arg(m_slave[i]->s_eep_man, 8, 16, QChar('0')));
                SetLog("info",QString("\t eep_id  = %1.").arg(m_slave[i]->s_eep_id, 8, 16, QChar('0')));
                SetLog("info",QString("\t name    = %1.").arg(QString(m_slave[i]->slave_name.c_str())));

                int rownum = ui->tableWidgetSlaves->rowCount();
                ui->tableWidgetSlaves->insertRow(rownum);

                if (m_slave[i]->s_eep_man == ECatDrv::ServoDevicedID_xServo
                         && (m_slave[i]->s_eep_id == ECatDrv::ServoProductCode_xServo_1
                                || m_slave[i]->s_eep_id == ECatDrv::ServoProductCode_xServo_2
                                || m_slave[i]->s_eep_id == ECatDrv::ServoProductCode_xServo_3
                                || m_slave[i]->s_eep_id == ECatDrv::ServoProductCode_xDrive))
                {
                    ui->tableWidgetSlaves->setItem(rownum, 0, new QTableWidgetItem(QString("%1_%2").arg(QString(m_slave[i]->slave_name.c_str())).arg(++DriveNum)));
                    // 目标速度
                    ui->tableWidgetSlaves->setItem(rownum, 5, new QTableWidgetItem(QString("10")));
                    ui->tableWidgetSlaves->item(rownum, 5)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
                    // 当前速度
                    ui->tableWidgetSlaves->setItem(rownum, 6, new QTableWidgetItem(QString("0")));
                    ui->tableWidgetSlaves->item(rownum, 6)->setTextAlignment(Qt::AlignCenter|Qt::AlignVCenter);
                }
                else
                {
                    ui->tableWidgetSlaves->setItem(rownum,0, new QTableWidgetItem(QString("%1").arg(QString(m_slave[i]->slave_name.c_str()))));
                }

                ui->tableWidgetSlaves->item(rownum, 0)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
                // 状态
                ui->tableWidgetSlaves->setItem(rownum, 1, new QTableWidgetItem(QString("Unknown")));
                ui->tableWidgetSlaves->item(rownum, 1)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
                // 伺服状态
                ui->tableWidgetSlaves->setItem(rownum, 2, new QTableWidgetItem(QString("Unknow")));
                ui->tableWidgetSlaves->item(rownum, 2)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
                // 版本
                ui->tableWidgetSlaves->setItem(rownum, 3, new QTableWidgetItem(QString("Unknow")));
                ui->tableWidgetSlaves->item(rownum, 3)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
                // St
                ui->tableWidgetSlaves->setItem(rownum, 4, new QTableWidgetItem(QString("Unknow")));
                ui->tableWidgetSlaves->item(rownum, 4)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);   
                // 产品号
                ui->tableWidgetSlaves->setItem(rownum, 7, new QTableWidgetItem(QString("Unknow")));
                ui->tableWidgetSlaves->item(rownum, 7)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
                // 版本号
                ui->tableWidgetSlaves->setItem(rownum, 8, new QTableWidgetItem(QString("Unknow")));
                ui->tableWidgetSlaves->item(rownum, 8)->setTextAlignment(Qt::AlignHCenter|Qt::AlignVCenter);
            }
            res = ecatMaster->MasterConfig(m_slave);
            if(res)
            {
                SetLog("error", QString("Ec_config failed."));
            }
            else
            {
                res = ecatMaster->SetSlavesToOp();
                if(res == 0)
                {
                    SetLog("info", QString("Set slaves to OP mode."));
                }                
                for (size_t i = 0; i < m_servo.size(); ++i)
                {
                    m_servo[i]->initPdoOutput();
                    m_servo[i]->initPdoInput();
                }
            }
            isServoInited = true;
            ui->pB_connect->setEnabled(false);
        }
        else if(res == -1)
        {
            SetLog("error", QString("Ec_init failed."));
        }
        else if(res == -2)
        {
            SetLog("error", QString("Slave not found."));
        }
        else if(res == -3)
        {
            SetLog("error", QString("At least one slave is not in safe_op."));
        }
        else if(res == -4)
        {
            SetLog("error", QString("The Master is Already Running."));
        }
    }
}

void MainWindow::pBSetOpSlot(void)
{
    if(!ecatMaster->m_masterInited)
    {
        QMessageBox::warning(this,"错误",QString("主站未初始化或未扫描到从站信息！"), QMessageBox::Yes, QMessageBox::Yes);
        return;
    }
    int res = ecatMaster->SetSlavesToOp();
    if(res == 0)
    {
        SetLog("info",QString("Set slaves to OP mode."));
    }
    else
    {
        QMessageBox::warning(this,"错误",QString("切换模式失败, res = %1!").arg(res),QMessageBox::Yes, QMessageBox::Yes);
    }
}

void MainWindow::pBSetSafeOpSlot(void)
{
    if(!ecatMaster->m_masterInited)
    {
        QMessageBox::warning(this,"错误",QString("主站未初始化或未扫描到从站信息！"),QMessageBox::Yes,QMessageBox::Yes);
        return;
    }
    int res = ecatMaster->SetSlavesToSafeOp();
    if(res == 0)
    {
        SetLog("info",QString("Set slaves to Safe OP mode."));
    }
    else
    {
        QMessageBox::warning(this,"错误",QString("切换模式失败, res = %1!").arg(res),QMessageBox::Yes,QMessageBox::Yes);
    }
}

void MainWindow::pBServoClearFaultSlot()
{
    if (!ecatMaster->m_masterInited)
    {
        QMessageBox::warning(this,"错误",QString("主站未初始化或未扫描到从站信息！"),QMessageBox::Yes,QMessageBox::Yes);
        return;
    }

    for (size_t i = 0; i < m_servo.size(); ++i)
    {
        m_servo[i]->ClearFault();
    }
}

void MainWindow::pBServoEnabledSlot()
{
    if(!ecatMaster->m_masterInited)
    {
        QMessageBox::warning(this, "错误", QString("主站未初始化或未扫描到从站信息！"), QMessageBox::Yes, QMessageBox::Yes);
        return;
    }

    if(ui->pB_servoEnabled->text().compare(QString("All Servo Enable")) == 0)
    {
        for (size_t i = 0; i < m_servo.size(); ++i)
        {
            if(!m_servo[i]->ServoEnable())
            {
                QMessageBox::warning(this, "错误", QString("上使能失败，主站先切到OP状态或者清错！"), QMessageBox::Yes,QMessageBox::Yes);
            }
        }
        ui->pB_servoEnabled->setText(QString("All Servo Disable"));
    }
    else
    {
        for (size_t i = 0; i < m_servo.size(); ++i)
        {
            m_servo[i]->ServoDisable();
        }
        ui->pB_servoEnabled->setText(QString("All Servo Enable"));
    }
}

void MainWindow::pBJogSlot()
{
    if(!ecatMaster->m_masterInited)
    {
        QMessageBox::warning(this, "错误", QString("主站未初始化或未扫描到从站信息！"), QMessageBox::Yes, QMessageBox::Yes);
        return;
    }

    if(ui->pB_Jog->text().compare(QString("Jog On")) == 0)
    {
        for (size_t i = 0; i < m_servo.size(); ++i)
        {
            size_t table_row = -1;
            for (size_t index = 0; index < m_slave.size(); ++index)
            {
                if (m_slave[index] == m_servo[i])
                {
                    table_row = index;
                    break;
                }
            }
//#ifndef xServo_SDO_CONTROL
//            if (m_servo[i]->m_state.stateVal != ECatDrv::ServoControlMode::OperationEnable)
//            {
//                QMessageBox::warning(this, "错误", QString("check all servo enable"), QMessageBox::Yes, QMessageBox::Yes);
//                continue;
//            }
//#endif
            m_servo[i]->JogOn(ui->tableWidgetSlaves->item(table_row, 5)->text().toInt());

        }
        ui->pB_Jog->setText(QString("Jog Off"));
    }
    else
    {
        for (size_t i = 0; i < m_servo.size(); ++i)
        {
            m_servo[i]->JogOff();
        }
        ui->pB_Jog->setText(QString("Jog On"));
    }
}

void MainWindow::timerServoLowUpdateSlot(void)
{
    if(!isServoInited)
    {
        return;  // 未初始化返回
    }

    // 获取主站及从站状态
    ecatMaster->GetAllSlaveState(m_slaveCurrentState);
    if(m_slaveCurrentState.size()>0)
    {
        if(m_slaveCurrentState[0] == ECatDrv::masterState::M_SAFE_OP)
        {
            ui->masterState->setText(QString("SafeOP"));
        }
        else if(m_slaveCurrentState[0] == ECatDrv::masterState::M_OP)
        {
            ui->masterState->setText(QString("OP"));
        }
        else if(m_slaveCurrentState[0] == ECatDrv::masterState::M_PRE_OP)
        {
            ui->masterState->setText(QString("PreOP"));
        }
        else
        {
            ui->masterState->setText(QString("Other"));
        }

        for(uint i = 1; i < m_slaveCurrentState.size(); i++)
        {
            if(m_slaveCurrentState[i] == ECatDrv::masterState::M_SAFE_OP)
            {
                ui->tableWidgetSlaves->item(i - 1, 1)->setText(QString("SafeOP"));
            }
            else if(m_slaveCurrentState[i] == ECatDrv::masterState::M_OP)
            {
                ui->tableWidgetSlaves->item(i - 1, 1)->setText(QString("OP"));
            }
            else if(m_slaveCurrentState[i] == ECatDrv::masterState::M_PRE_OP)
            {
                ui->tableWidgetSlaves->item(i - 1, 1)->setText(QString("PreOP"));
            }
            else
            {
                ui->tableWidgetSlaves->item(i - 1, 1)->setText(QString("Other"));
            }
        }
    }


    if(ui->tableWidgetSlaves->rowCount() > 0)
    {
        // 从站状态更新
        for(uint i = 0; i < m_servo.size(); ++i)
        {
            m_servo[i]->StMachineUpdate();
            // 数据显示
            size_t table_row = -1;
            for (size_t index = 0; index < m_slave.size(); ++index)
            {
                if (m_slave[index] == m_servo[i])
                {
                    table_row = index;
                    break;
                }
            }

            if (table_row == (size_t)-1)
            {
                SetLog(ERROR, "该伺服不存在于从站列表中");
                continue;
            }

            uint16_t state;
            uint8_t op;
            QString version = QString::fromStdString(m_servo[i]->GetVersionStr());
            bool isBootloader = m_servo[i]->isBootloaderState();
            m_servo[i]->GetSlaveState(state, op);

            ui->tableWidgetSlaves->item(table_row, 3)->setText(version);
            ui->tableWidgetSlaves->item(table_row, 4)->setText(QString("0x%1").arg(state, 4, 16, QChar('0')));

            ui->tableWidgetSlaves->item(table_row, 6)->setText(QString("%1").arg(-m_servo[i]->m_state.velocity));
            ui->tableWidgetSlaves->item(table_row, 7)->setText(QString("0x%1").arg(ec_slave[table_row + 1].eep_id, 4, 16, QChar('0')));
            ui->tableWidgetSlaves->item(table_row, 8)->setText(QString("0x%1").arg(ec_slave[table_row + 1].eep_rev, 4, 16, QChar('0')));


            uint16_t faultCode;
            static uint16_t fault_old;
            std::string errStr;
            m_servo[i]->GetFaultInfo(faultCode, errStr);
            if (faultCode == 0)
            {
                if(isBootloader)
                {
                    ui->tableWidgetSlaves->item(table_row, 2)->setText(QString("Bootloader"));
                }
                else
                {
                    ui->tableWidgetSlaves->item(table_row, 2)->setText(QString("正常"));
                }
                if(fault_old != faultCode)
                {
                    SetLog("info",QString("clear error success"));
                    fault_old = faultCode;
                }
            }
            else
            {
                ui->tableWidgetSlaves->item(table_row, 2)->setText(QString("%1").arg(faultCode, 4, 16, QChar('0')));
                if(fault_old != faultCode)
                {
                    SetLog("error",QString(errStr.c_str()));
                    fault_old = faultCode;
                }
            }
        }
    }
}


// for log
void MainWindow::SetTextLineColor(QColor color)
{
    QTextCursor cursor = ui->plainTextEditLog->textCursor();
    cursor.movePosition(QTextCursor::End);  // cursor移至末尾
    cursor.select(QTextCursor::LineUnderCursor);   //选中要着色的内容
    QTextCharFormat fmt;
    fmt.setForeground(color);
    cursor.mergeCharFormat(fmt);    //设置文本格式
    cursor.clearSelection();        //撤销选中
    cursor.movePosition(QTextCursor::EndOfLine);  //cursor和anchor均移至末尾
}

void MainWindow::SetLog(QString type, QString str)
{
    if(type == QString("warning"))
    {
        ui->plainTextEditLog->appendPlainText(QString("[warn]: ") + str);
        SetTextLineColor(QColor(200, 200, 0));
    }
    else if(type == QString("error"))
    {
        ui->plainTextEditLog->appendPlainText(QString("[erro]: ") + str);
        SetTextLineColor(QColor(Qt::red));
    }
    else if(type == QString("info"))
    {
        ui->plainTextEditLog->appendPlainText(QString("[info]: ") + str);
        SetTextLineColor(QColor(Qt::darkGreen));
    }

}
